#!/usr/bin/env python3
import rospy  
from std_msgs.msg import String
from std_msgs.msg import Int32
from mavproxy_ros1.msg import FlightCmd

# 根据此表映射指令，然后将后续所有字符串全部以空格分割作为参数识别
FLIGHT_CMDS = [
    "flight_takeoff" ,
    "flight_land" ,
    "flight_hover" ,
    "flight_targetpos" ,
    "flight_targetpos_h",
    "flight_speedup",
    "flight_speeddown",
    "flight_moveforward",
    "flight_moveback",
    "flight_movehorizontal",
    "flight_movevertical",
    "flight_rotatehorizontal",
    "flight_planadd",
    "flight_plandel",
    "flight_planedit"
]

def check_cmd_match(cmd_input):
    match_flag = False
    for cmd_type in FLIGHT_CMDS:  
        if cmd_type.lower() in cmd_input:
            match_flag = True
            return (match_flag, cmd_type)
        else:
            continue
    return (match_flag, "")

def text_to_command_publisher():  
    while not rospy.is_shutdown():  
        user_input = input("Flight cmd input: ")

        ret = check_cmd_match(user_input)
        if ret[0]:
            get_flight_params(ret[1], user_input)
        else:
            continue

def get_flight_params(command_type, command_full):
    clean_command = ",".join(command_full.strip().split())
    # params_list = [Int32(x) for x in clean_command.split(",")[1:] ]
    params_list = [int(x) for x in clean_command.split(",")[1:] ]
    rospy.loginfo(params_list)

    # 根据命令类型，次序填入参数，封装到消息中
    currentCmd = FlightCmd()
    currentCmd.cmdtype = command_type
    currentCmd.addedcmd = ""
    currentCmd.paramdata = params_list
    command_pub.publish(currentCmd)
    
    pass

def get_takeoff_cmd(command_type):
    pass
  
if __name__ == '__main__':  
    try:
        rospy.init_node('flight_cmd_parser', anonymous=True)
        command_pub = rospy.Publisher('/mavproxy_ros1/parsed_flight_cmd', FlightCmd, queue_size=10)
        text_to_command_publisher()  
    except rospy.ROSInterruptException:
        pass